#include "bmp280.h"

esp_err_t bmp280_read_reg(i2c_master_dev_handle_t dev_handle, uint8_t *reg, uint8_t *data, uint16_t len)
{
    return i2c_master_transmit_receive(dev_handle, reg, 1, data, len, -1);
}
esp_err_t bmp280_write_reg(i2c_master_dev_handle_t dev_handle, uint8_t *reg, uint8_t *data)
{
    uint8_t buffer[2];
    buffer[0] = *reg;
    buffer[1] = *data;
    return i2c_master_transmit(dev_handle, buffer, 2, -1);
}

i2c_master_dev_handle_t bmp280_init(i2c_master_bus_handle_t i2c_bus,BMP280_Object_t *pObj)
{
    uint8_t reg,temple;
    /* Add Sensor to I2C Bus */
    i2c_device_config_t bmp280_cfg = {
        .dev_addr_length = I2C_ADDR_BIT_LEN_7,
        .device_address = pObj->Addr,
        .scl_speed_hz = pObj->SCL_Speed,
    };
    i2c_master_dev_handle_t bmp280_handle;
    ESP_ERROR_CHECK_WITHOUT_ABORT(i2c_master_bus_add_device(i2c_bus, &bmp280_cfg, &bmp280_handle));
    /* Check Sensor ID */
    ESP_ERROR_CHECK_WITHOUT_ABORT(bmp280_read_id(bmp280_handle));
    /* Get Calibration Data */
    ESP_ERROR_CHECK_WITHOUT_ABORT(bmp280_get_calibration_data(bmp280_handle, pObj, &bmp280_calidata));
    /* Soft Reset */
    ESP_ERROR_CHECK_WITHOUT_ABORT(bmp280_reset(bmp280_handle));
    /* Initialize Work Mode */
    reg = BMP280_CTRL_MEAS;
    pObj->PowerMode = BMP280_NORMAL_MODE;
    pObj->Temp_os = BMP280_TEMP_OVERSAMPLE_X1;
    pObj->Press_os = BMP280_PRESS_OVERSAMPLE_X4;
    temple = pObj->Temp_os + pObj->Press_os + pObj->PowerMode;
    ESP_ERROR_CHECK_WITHOUT_ABORT(bmp280_write_reg(bmp280_handle,&reg,&temple));
    reg = BMP280_CONFIG;
    pObj->StandByTime = BMP280_STANDBY_TIME_0ms5;
    pObj->FilterMode = BMP280_FILTER_OFF;
    pObj->Spi3State = BMP280_SPI3W_OFF;
    temple = pObj->StandByTime + pObj->FilterMode + pObj->Spi3State;
    ESP_ERROR_CHECK_WITHOUT_ABORT(bmp280_write_reg(bmp280_handle,&reg,&temple));

    pObj->is_initialized = 1;
    
    return bmp280_handle;
}
esp_err_t bmp280_deinit(i2c_master_dev_handle_t dev_handle,BMP280_Object_t *pObj)
{
    pObj->Temp_os = BMP280_PRESS_OVERSAMPLE_SKIP;
    pObj->Press_os = BMP280_PRESS_OVERSAMPLE_SKIP;
    pObj->is_initialized = 0;
    return i2c_master_bus_rm_device(dev_handle);
}
esp_err_t bmp280_reset(i2c_master_dev_handle_t dev_handle)
{
    uint8_t reg,temple;
    reg = BMP280_RESET_REG;
    temple = BMP280_RESET_VALUE;
    return bmp280_write_reg(dev_handle,&reg,&temple);
}

esp_err_t bmp280_read_id(i2c_master_dev_handle_t dev_handle)
{
    uint8_t reg,temple;
    reg = BMP280_ID_REG;
    ESP_ERROR_CHECK_WITHOUT_ABORT(bmp280_read_reg(dev_handle,&reg,&temple,1));
    if(temple==BMP280_ID) return ESP_OK;
    else return ESP_FAIL;
}
esp_err_t bmp280_read_status(i2c_master_dev_handle_t dev_handle)
{
    uint8_t temple,reg;
    reg = BMP280_STATUS;
    ESP_ERROR_CHECK_WITHOUT_ABORT(bmp280_read_reg(dev_handle,&reg,&temple,1));
    temple = temple|0b11110110;
    printf("%d ",temple);
    if(temple==0b11110110) return ESP_OK;
    else return ESP_FAIL;
}

esp_err_t bmp280_read_mode(i2c_master_dev_handle_t dev_handle,BMP280_Object_t *pObj)
{
    uint8_t temple,reg;
    reg = BMP280_CTRL_MEAS;
    uint8_t value;
    if(bmp280_read_reg(dev_handle,&reg,&temple,1)!=ESP_OK) return ESP_FAIL;
    value = temple&0b11100000;
    pObj->Temp_os = value;
    value = temple&0b00011100;
    pObj->Press_os = value;
    value = temple&0b00000011;
    pObj->PowerMode = value;
    return ESP_OK;
}
esp_err_t bmp280_set_mode(i2c_master_dev_handle_t dev_handle,BMP280_Object_t *pObj,BMP280_Operating_Mode_t power_mode,BMP280_osrs_t_t temp_oversample,BMP280_osrs_p_t press_oversample)
{
    uint8_t temple,reg;
    reg = BMP280_CTRL_MEAS;
    temple = temp_oversample+press_oversample+power_mode;
    pObj->Temp_os = temp_oversample;
    pObj->Press_os = press_oversample;
    pObj->PowerMode = power_mode;
    return bmp280_write_reg(dev_handle,&reg,&temple);
}
esp_err_t bmp280_read_config(i2c_master_dev_handle_t dev_handle,BMP280_Object_t *pObj)
{
    uint8_t temple,reg;
    reg = BMP280_CONFIG;
    uint8_t value;
    if(bmp280_read_reg(dev_handle,&reg,&temple,1)!=ESP_OK) return ESP_FAIL;
    value = temple&0b11100000;
    pObj->StandByTime = value;
    value = temple&0b00011100;
    pObj->FilterMode = value;
    value = temple&0b00000001;
    pObj->Spi3State = value;
    return ESP_OK;
}
esp_err_t bmp280_set_config(i2c_master_dev_handle_t dev_handle,BMP280_Object_t *pObj,BMP280_t_sb_t tsb_cfg,BMP280_filter_t filter_cfg,BMP280_spi3w_en_t spi3w_cfg)
{
    uint8_t temple,reg;
    reg = BMP280_CONFIG;
    temple = tsb_cfg+filter_cfg+spi3w_cfg;
    pObj->StandByTime = tsb_cfg;
    pObj->FilterMode = filter_cfg;
    pObj->Spi3State = spi3w_cfg;
    return bmp280_write_reg(dev_handle,&reg,&temple);
}

esp_err_t bmp280_get_calibration_data(i2c_master_dev_handle_t dev_handle,BMP280_Object_t *pObj,BMP280_Cali_Data_t *data_t)
{
    uint8_t reg;
    reg = BMP280_DIG_T1_LSB_REG;
    /* Write the address of reading reg first*/
    ESP_ERROR_CHECK_WITHOUT_ABORT(i2c_master_transmit(dev_handle,&reg,1,-1));
    uint8_t buffer[24];
    ESP_ERROR_CHECK_WITHOUT_ABORT(i2c_master_receive(dev_handle,buffer,24,-1));
    data_t->dig_T1 = ((short)buffer[1])<<8 | (short)buffer[0];
    data_t->dig_T2 = ((short)buffer[3])<<8 | (short)buffer[2];
    data_t->dig_T3 = ((short)buffer[5])<<8 | (short)buffer[4];
    data_t->dig_P1 = ((short)buffer[7])<<8 | (short)buffer[6];
    data_t->dig_P2 = ((short)buffer[9])<<8 | (short)buffer[8];
    data_t->dig_P3 = ((short)buffer[11])<<8 | (short)buffer[10];
    data_t->dig_P4 = ((short)buffer[13])<<8 | (short)buffer[12];
    data_t->dig_P5 = ((short)buffer[15])<<8 | (short)buffer[14];
    data_t->dig_P6 = ((short)buffer[17])<<8 | (short)buffer[16];
    data_t->dig_P7 = ((short)buffer[19])<<8 | (short)buffer[18];
    data_t->dig_P8 = ((short)buffer[21])<<8 | (short)buffer[20];
    data_t->dig_P9 = ((short)buffer[23])<<8 | (short)buffer[22];

    return ESP_OK;
}
esp_err_t bmp280_read_press(i2c_master_dev_handle_t dev_handle,BMP280_Object_t *pObj,BMP280_Data_t *data_t)
{
    if(bmp280_read_status(dev_handle)!=ESP_OK) return ESP_ERR_INVALID_STATE;
    if(pObj->Press_os==BMP280_PRESS_OVERSAMPLE_SKIP) return ESP_ERR_NOT_ALLOWED;
    uint8_t reg = BMP280_PRESS_MSB;
    uint8_t buffer[3];/*MSB LSB XLSB*/
    ESP_ERROR_CHECK_WITHOUT_ABORT(i2c_master_transmit(dev_handle,&reg,1,-1));
    ESP_ERROR_CHECK_WITHOUT_ABORT(i2c_master_receive(dev_handle,buffer,3,-1));
    data_t->ADC_P = ((int32_t)buffer[0])<<12 | ((int32_t)buffer[1])<<4 | (int32_t)buffer[2];
    return ESP_OK;
}
esp_err_t bmp280_read_temp(i2c_master_dev_handle_t dev_handle,BMP280_Object_t *pObj,BMP280_Data_t *data_t)
{
    if(bmp280_read_status(dev_handle)!=ESP_OK) return ESP_ERR_INVALID_STATE;
    if(pObj->Temp_os==BMP280_TEMP_OVERSAMPLE_SKIP) return ESP_ERR_NOT_ALLOWED;
    uint8_t reg = BMP280_TEMP_MSB;
    uint8_t buffer[3];/*MSB LSB XLSB*/
    ESP_ERROR_CHECK_WITHOUT_ABORT(i2c_master_transmit(dev_handle,&reg,1,-1));
    ESP_ERROR_CHECK_WITHOUT_ABORT(i2c_master_receive(dev_handle,buffer,3,-1));
    data_t->ADC_T = ((int32_t)buffer[0])<<12 | ((int32_t)buffer[1])<<4 | (int32_t)buffer[2];
    return ESP_OK;
}
esp_err_t bmp280_read_data(i2c_master_dev_handle_t dev_handle,BMP280_Object_t *pObj,BMP280_Data_t *data_t)
{
    /* Check state */
    if(bmp280_read_status(dev_handle)!=ESP_OK) return ESP_ERR_INVALID_STATE;
    /* Get Temp ADC */
    uint8_t reg = BMP280_TEMP_MSB;
    uint8_t buffer[3];/*MSB LSB XLSB*/
    ESP_ERROR_CHECK_WITHOUT_ABORT(i2c_master_transmit(dev_handle,&reg,1,-1));
    ESP_ERROR_CHECK_WITHOUT_ABORT(i2c_master_receive(dev_handle,buffer,3,-1));
    data_t->ADC_T = ((int32_t)buffer[0])<<12 | ((int32_t)buffer[1])<<4 | ((int32_t)buffer[2])>>4;
    /* Caculate Temp */
    int32_t var1_T,var2_T,T;
    var1_T = (((data_t->ADC_T>>3)-((int32_t)bmp280_calidata.dig_T1<<1)) * ((int32_t)bmp280_calidata.dig_T2))>>11;
    var2_T = (((((data_t->ADC_T >> 4) - ((int32_t)bmp280_calidata.dig_T1)) *((data_t->ADC_T >> 4) - ((int32_t)bmp280_calidata.dig_T1))) >>12) *((int32_t)bmp280_calidata.dig_T3)) >>14;
    data_t->t_fine = var1_T+var2_T;
    T = (data_t->t_fine*5+128)>>8;
    data_t->Temp = (float)T/100;
    /* Get Press ADC */
    reg = BMP280_PRESS_MSB;
    ESP_ERROR_CHECK_WITHOUT_ABORT(i2c_master_transmit(dev_handle,&reg,1,-1));
    ESP_ERROR_CHECK_WITHOUT_ABORT(i2c_master_receive(dev_handle,buffer,3,-1));
    data_t->ADC_P = ((int32_t)buffer[0])<<12 | ((int32_t)buffer[1])<<4 | ((int32_t)buffer[2])>>4;
    /* Caculate Press */
    int64_t var1_P,var2_P,P;
    var1_P = (int64_t)data_t->t_fine - 128000;
    var2_P = var1_P * var1_P * (int64_t)bmp280_calidata.dig_P6;
    var2_P = var2_P + ((var1_P*(int64_t)bmp280_calidata.dig_P5)<<17);
    var2_P = var2_P + (((int64_t)bmp280_calidata.dig_P4)<<35);
    var1_P = ((var1_P * var1_P * (int64_t)bmp280_calidata.dig_P3)>>8) + ((var1_P * (int64_t)bmp280_calidata.dig_P2)<<12);
    var1_P = (((((int64_t)1)<<47)+var1_P)) * ((int64_t)bmp280_calidata.dig_P1)>>33;
    if(var1_P!=0)
    {
        P = 1048576-data_t->ADC_P;
        P = (((P<<31)-var2_P)*3125)/var1_P;
        var1_P = (((int64_t)bmp280_calidata.dig_P9) * (P>>13) * (P>>13))>>25;
        var2_P = (((int64_t)bmp280_calidata.dig_P8) * P)>>19;
        P = ((P+var1_P+var2_P)>>8) + (((int64_t)bmp280_calidata.dig_P7)<<4);
        data_t->Press = (float)P/256;
    }
    return ESP_OK;
}